// Harris_corner_detection.cpp : 定义控制台应用程序的入口点。
//
//-----------------------------------【头文件包含部分】---------------------------------------  
//      描述：包含程序所依赖的头文件  
//---------------------------------------------------------------------------------------------- 
#include "stdafx.h" 
#include <stdio.h>
#include <iostream>
#include <vector>
#include <corecrt_search.h>
#include <math.h>
#include <opencv2/opencv.hpp>  
#include <opencv2/imgproc/imgproc.hpp>  

//-----------------------------------【命名空间声明部分】---------------------------------------  
//      描述：包含程序所使用的命名空间  
//-----------------------------------------------------------------------------------------------   
using namespace cv;
using namespace std;


double  Dist(double a, double b, double c, double d)
{
    double distance = sqrt((a - c)*(a - c) + (b - d)*(b - d));
	return(distance);
}
struct Bian
{
	double bian[];
	//size_t index[];
}bian2, bian;

//static Bian bian2,bian;
Bian Cr(vector<Point2f>corners,Mat img)
{
	//cr_element cr_ele;
	double x[8];
	double y[8];
	double cr[8][6];
	int point_ind[4],ai,bi,ci,di;
	double L[4];
	double bias;
	double  min_bias=0;
	//Bili bili;
	//double* bili = new double[2];
	//double bili[2];
	//double bian[4];
	/*提取角点坐标*/
	for (int i = 0; i < 8; i++)
	{
		x[i] = corners[i].x;
		y[i] = corners[i].y;
	}
	for (int i = 0; i<8; i++)
		for (int j = i + 1; j<8; j++)
			for (int k = j + 1; k<8; k++)
				for (int l = k + 1; l<8; l++)
				{
					double deta_x = x[j] - x[i];
					double deta_y = y[i] - y[j];
					double cross = y[i] * x[j] - x[i] * y[j];
					double L_C = deta_y*x[k] + deta_x*y[k];
					double L_D = deta_y*x[l] + deta_x*y[l];
					bias = (L_C - cross)*(L_C - cross) + (L_D - cross)*(L_D - cross);
					if (min_bias>bias||min_bias==0)
					{
						min_bias = bias;
						point_ind[0] = i;
						point_ind[1] = j;
						point_ind[2] = k;
						point_ind[3] = l;
					}
				}
	int max_dist[2];
	int min_dist[2];

	/*find the two couple of points,*/
	for (int i = 0; i < 4; i++)
		for (int j = i + 1; j < 4; j++)
		{
			double min = 0;
			double max = 0;
			double dist = sqrt((x[point_ind[i]] - x[j])*(x[point_ind[i]] - x[point_ind[j]]) + (y[point_ind[i]] - y[point_ind[j]])*(y[point_ind[i]] - y[point_ind[j]]));
			if (max < dist)
			{
				max = dist;
				max_dist[0] = point_ind[i];
				max_dist[1] = point_ind[j];
			}
			if (min > dist || min == 0)
			{
				min = dist;
				min_dist[0] = point_ind[i];
				min_dist[1] = point_ind[j];

			}
		}



	ai=point_ind[0];
	bi=point_ind[1];
	ci=point_ind[2];
	di=point_ind[3];
	Mat image = img;
	imshow("1", image);
	RNG  rng(12345);
	for (int i = 0; i < 4;i++)
	circle(image, corners[point_ind[i]], 1, Scalar(rng.uniform(0, 255), rng.uniform(0, 255), rng.uniform(0, 255)), 2, 8, 0);
	imshow("2", image);
//	circle(img,corners[ai],1,)
	//printf("%f\n", min_bias);
	double d_l,d_r;
	double deta_x = x[di] - x[ai];
	double deta_y = y[ai] - y[di];
	double cross = y[ai] * x[di] - x[ai] * y[di];
	double dist23_1, dist23_2;
	int ind18[2];
	int ind180,ind181;

	int ind45[2];
	int ind450,ind451;


	double dist18[4];
	double dist45[4];
	for (int i = 0; i < 8; i++)
		for(int j = i+1; j < 8; j++)
	{
		if (i != ai&&i != bi&&i != ci&&i != di&&j != ai&&j != bi&&j != ci&&j != di)
		{
			double judgement_i = deta_y*x[i] + deta_x*y[i] - cross;
			double judgement_j = deta_y*x[j] + deta_x*y[j] - cross;
			if (judgement_i > 0 && judgement_j > 0)
			{
				printf("j_i=%f,j_j=%f\n", judgement_i, judgement_j);

				//d_r = abs(judgement_i + judgement_j) / 2;
				ind180 = i;
				ind181 = j;
				//d_r = max(abs(judgement_i), abs(judgement_j));
				//bian.bian[3] = sqrt((x[i] - x[j])*(x[i] - x[j])+ (y[i] - y[j])*(y[i] - y[j]));
				 dist23_1= sqrt((x[i] - x[j])*(x[i] - x[j])+ (y[i] - y[j])*(y[i] - y[j]));

				//bian.bian[0] = abs(d_r)/sqrt(deta_x*deta_x+deta_y*deta_y);
				
				circle(image, corners[i], 1, Scalar(rng.uniform(0, 255), rng.uniform(0, 255), rng.uniform(0, 255)), 2, 8, 0);
				circle(image, corners[j], 1, Scalar(rng.uniform(0, 255), rng.uniform(0, 255), rng.uniform(0, 255)), 2, 8, 0);
				imshow("3", image);
				
				//printf("%f ", d_r);
			}

			else if(judgement_i<0&&judgement_j<0)
			{
				printf("j_i=%f,j_j=%f\n", judgement_i, judgement_j);

				//d_l = abs(judgement_i + judgement_j) / 2;
				//d_l = max(abs(judgement_i) , abs(judgement_j));
				//bian.bian[2]= sqrt((x[i] - x[j])*(x[i] - x[j])+ (y[i] - y[j])*(y[i] - y[j]));
				//bian.bian[1] = abs(d_l)/sqrt(deta_x*deta_x+deta_y*deta_y);
				dist23_2 = sqrt((x[i] - x[j])*(x[i] - x[j]) + (y[i] - y[j])*(y[i] - y[j]));
				ind450 = i;
				ind451 = j;


				circle(image, corners[i], 1, Scalar(rng.uniform(0, 255), rng.uniform(0, 255), rng.uniform(0, 255)), 2, 8, 0);
				circle(image, corners[j], 1, Scalar(rng.uniform(0, 255), rng.uniform(0, 255), rng.uniform(0, 255)), 2, 8, 0);
				imshow("4", image);
				
				//printf("%f\n", d_l);
			}
			//else return 0;
		}
	}
	/*
	if (bian.bian[2] > bian.bian[3])
	{
		double bb = bian.bian[3];
		bian.bian[3] = bian.bian[2];
		bian.bian[2] = bb;
		double aa = bian.bian[0];
		bian.bian[0] = bian.bian[1];
		bian.bian[1] = aa;
	}
	printf("%f,%f,%f,%f\n", bian.bian[0], bian.bian[1], bian.bian[2], bian.bian[3]);
	*/

	if (dist23_1 > dist23_2)
	{
		ind18[0] = ind180;
		ind18[1] = ind181;
		ind45[0] = ind450;
		ind45[1] = ind451;
		bian.bian[3] = dist23_1;
		bian.bian[2] = dist23_2;
		//double l[4];
		for (int i = 0; i < 2; i++)
			for (int j = 0; j < 2; j++)
			{
				dist18[i*2+j] = Dist(x[ind18[i]], y[ind18[i]], x[max_dist[j]], y[max_dist[j]]);
					//= sqrt(x[ind18[i]] - x[max_dist[j]]* x[ind18[i]] - x[max_dist[j]]+ y[ind18[i]] - y[max_dist[j]]);
			}
		for (int i = 0; i < 2; i++)
			for (int j = 0; j < 2; j++)
			{
				dist45[i * 2 + j] = Dist(x[ind45[i]], y[ind45[i]], x[min_dist[j]], y[min_dist[j]]);
				//= sqrt(x[ind18[i]] - x[max_dist[j]]* x[ind18[i]] - x[max_dist[j]]+ y[ind18[i]] - y[max_dist[j]]);
			}
		//double deta_dist18;
		for (int k = 0; k < 4; k++)
			//for (int m = k + 1; m < 4;m++)
		{
			double d1 = 0;
			//double d2 = 0;
			if(d1==0||d1>dist18[k])
			{
				d1 = dist18[k];
			}
			bian.bian[0] = d1;
			double d45 = 0;
			d45+= dist45[k];
			bian.bian[1] = d45 / 4;
		}
		
	}
	else if (dist23_1 < dist23_2)
	{
		ind18[0] = ind450;
		ind18[1] = ind451;
		ind45[0] = ind180;
		ind45[1] = ind181;
		bian.bian[3] = dist23_2;
		bian.bian[2] = dist23_1;
		//double l[4];
		for (int i = 0; i < 2; i++)
			for (int j = 0; j < 2; j++)
			{
				dist18[i * 2 + j] = Dist(x[ind18[i]], y[ind18[i]], x[max_dist[j]], y[max_dist[j]]);
				//= sqrt(x[ind18[i]] - x[max_dist[j]]* x[ind18[i]] - x[max_dist[j]]+ y[ind18[i]] - y[max_dist[j]]);
			}
		for (int i = 0; i < 2; i++)
			for (int j = 0; j < 2; j++)
			{
				dist45[i * 2 + j] = Dist(x[ind45[i]], y[ind45[i]], x[min_dist[j]], y[min_dist[j]]);
				//= sqrt(x[ind18[i]] - x[max_dist[j]]* x[ind18[i]] - x[max_dist[j]]+ y[ind18[i]] - y[max_dist[j]]);
			}
		//double deta_dist18;
		for (int k = 0; k < 4; k++)
			//for (int m = k + 1; m < 4;m++)
		{
			double d1 = 0;
			//double d2 = 0;
			if (d1 == 0 || d1 > dist18[k])
			{
				d1 = dist18[k];
			}
			bian.bian[0] = d1;
			double d45 = 0;
			d45 += dist45[k];
			bian.bian[1] = d45 / 4;
		}
	}




	return bian;
}
/**/
Bian Cr2(vector<Point2f>corners, Mat img)
{
	//cr_element cr_ele;
	double x[8];
	double y[8];
	double cr[8][6];
	int point_ind[4], ai, bi, ci, di;
	double L[4];
	double bias;
	double  min_bias = 0;
	//Bili bili;
	//double* bili = new double[2];
	//double bili[2];
	//double bian[4];
	/*提取角点坐标*/
	for (int i = 0; i < 8; i++)
	{
		x[i] = corners[i].x;
		y[i] = corners[i].y;
	}
	for (int i = 0; i<8; i++)
		for (int j = i + 1; j<8; j++)
			for (int k = j + 1; k<8; k++)
				for (int l = k + 1; l<8; l++)
				{
					double deta_x = x[j] - x[i];
					double deta_y = y[i] - y[j];
					double cross = y[i] * x[j] - x[i] * y[j];
					double L_C = deta_y*x[k] + deta_x*y[k];
					double L_D = deta_y*x[l] + deta_x*y[l];
					bias = (L_C - cross)*(L_C - cross) + (L_D - cross)*(L_D - cross);
					if (min_bias>bias || min_bias == 0)
					{
						min_bias = bias;
						point_ind[0] = i;
						point_ind[1] = j;
						point_ind[2] = k;
						point_ind[3] = l;
					}
				}
	int max_dist[2];
	int min_dist[2];

	/*find the two couple of points,*/
	for (int i = 0; i < 4; i++)
		for (int j = i + 1; j < 4; j++)
		{
			double min = 0;
			double max = 0;
			double dist = sqrt((x[point_ind[i]] - x[j])*(x[point_ind[i]] - x[point_ind[j]]) + (y[point_ind[i]] - y[point_ind[j]])*(y[point_ind[i]] - y[point_ind[j]]));
			if (max < dist)
			{
				max = dist;
				max_dist[0] = point_ind[i];
				max_dist[1] = point_ind[j];
			}
			if (min > dist || min == 0)
			{
				min = dist;
				min_dist[0] = point_ind[i];
				min_dist[1] = point_ind[j];

			}
		}



	ai = point_ind[0];
	bi = point_ind[1];
	ci = point_ind[2];
	di = point_ind[3];
	Mat image = img;
	imshow("1", image);
	RNG  rng(12345);
	for (int i = 0; i < 4; i++)
		circle(image, corners[point_ind[i]], 1, Scalar(rng.uniform(0, 255), rng.uniform(0, 255), rng.uniform(0, 255)), 2, 8, 0);
	imshow("2", image);
	//	circle(img,corners[ai],1,)
	//printf("%f\n", min_bias);
	double d_l, d_r;
	double deta_x = x[di] - x[ai];
	double deta_y = y[ai] - y[di];
	double cross = y[ai] * x[di] - x[ai] * y[di];
	double dist23_1, dist23_2;
	int ind18[2];
	int ind180, ind181;

	int ind45[2];
	int ind450, ind451;


	double dist18[4];
	double dist45[4];
	for (int i = 0; i < 8; i++)
		for (int j = i + 1; j < 8; j++)
		{
			if (i != ai&&i != bi&&i != ci&&i != di&&j != ai&&j != bi&&j != ci&&j != di)
			{
				double judgement_i = deta_y*x[i] + deta_x*y[i] - cross;
				double judgement_j = deta_y*x[j] + deta_x*y[j] - cross;
				if (judgement_i > 0 && judgement_j > 0)
				{
					printf("j_i=%f,j_j=%f\n", judgement_i, judgement_j);

					//d_r = abs(judgement_i + judgement_j) / 2;
					ind180 = i;
					ind181 = j;
					//d_r = max(abs(judgement_i), abs(judgement_j));
					//bian.bian[3] = sqrt((x[i] - x[j])*(x[i] - x[j])+ (y[i] - y[j])*(y[i] - y[j]));
					dist23_1 = sqrt((x[i] - x[j])*(x[i] - x[j]) + (y[i] - y[j])*(y[i] - y[j]));

					//bian.bian[0] = abs(d_r)/sqrt(deta_x*deta_x+deta_y*deta_y);

					circle(image, corners[i], 1, Scalar(rng.uniform(0, 255), rng.uniform(0, 255), rng.uniform(0, 255)), 2, 8, 0);
					circle(image, corners[j], 1, Scalar(rng.uniform(0, 255), rng.uniform(0, 255), rng.uniform(0, 255)), 2, 8, 0);
					imshow("5", image);

					//printf("%f ", d_r);
				}

				else if (judgement_i<0 && judgement_j<0)
				{
					printf("j_i=%f,j_j=%f\n", judgement_i, judgement_j);

					//d_l = abs(judgement_i + judgement_j) / 2;
					//d_l = max(abs(judgement_i) , abs(judgement_j));
					//bian.bian[2]= sqrt((x[i] - x[j])*(x[i] - x[j])+ (y[i] - y[j])*(y[i] - y[j]));
					//bian.bian[1] = abs(d_l)/sqrt(deta_x*deta_x+deta_y*deta_y);
					dist23_2 = sqrt((x[i] - x[j])*(x[i] - x[j]) + (y[i] - y[j])*(y[i] - y[j]));
					ind450 = i;
					ind451 = j;


					circle(image, corners[i], 1, Scalar(rng.uniform(0, 255), rng.uniform(0, 255), rng.uniform(0, 255)), 2, 8, 0);
					circle(image, corners[j], 1, Scalar(rng.uniform(0, 255), rng.uniform(0, 255), rng.uniform(0, 255)), 2, 8, 0);
					imshow("6", image);

					//printf("%f\n", d_l);
				}
				//else return 0;
			}
		}
	/*
	if (bian.bian[2] > bian.bian[3])
	{
	double bb = bian.bian[3];
	bian.bian[3] = bian.bian[2];
	bian.bian[2] = bb;
	double aa = bian.bian[0];
	bian.bian[0] = bian.bian[1];
	bian.bian[1] = aa;
	}
	printf("%f,%f,%f,%f\n", bian.bian[0], bian.bian[1], bian.bian[2], bian.bian[3]);
	*/

	if (dist23_1 > dist23_2)
	{
		ind18[0] = ind180;
		ind18[1] = ind181;
		ind45[0] = ind450;
		ind45[1] = ind451;
		bian2.bian[3] = dist23_1;
		bian2.bian[2] = dist23_2;
		//double l[4];
		for (int i = 0; i < 2; i++)
			for (int j = 0; j < 2; j++)
			{
				dist18[i * 2 + j] = Dist(x[ind18[i]], y[ind18[i]], x[max_dist[j]], y[max_dist[j]]);
				//= sqrt(x[ind18[i]] - x[max_dist[j]]* x[ind18[i]] - x[max_dist[j]]+ y[ind18[i]] - y[max_dist[j]]);
			}
		for (int i = 0; i < 2; i++)
			for (int j = 0; j < 2; j++)
			{
				dist45[i * 2 + j] = Dist(x[ind45[i]], y[ind45[i]], x[min_dist[j]], y[min_dist[j]]);
				//= sqrt(x[ind18[i]] - x[max_dist[j]]* x[ind18[i]] - x[max_dist[j]]+ y[ind18[i]] - y[max_dist[j]]);
			}
		//double deta_dist18;
		for (int k = 0; k < 4; k++)
			//for (int m = k + 1; m < 4;m++)
		{
			double d1 = 0;
			//double d2 = 0;
			if (d1 == 0 || d1>dist18[k])
			{
				d1 = dist18[k];
			}
			bian2.bian[0] = d1;
			double d45 = 0;
			d45 += dist45[k];
			bian2.bian[1] = d45 / 4;
		}

	}
	else if (dist23_1 < dist23_2)
	{
		ind18[0] = ind450;
		ind18[1] = ind451;
		ind45[0] = ind180;
		ind45[1] = ind181;
		bian2.bian[3] = dist23_2;
		bian2.bian[2] = dist23_1;
		//double l[4];
		for (int i = 0; i < 2; i++)
			for (int j = 0; j < 2; j++)
			{
				dist18[i * 2 + j] = Dist(x[ind18[i]], y[ind18[i]], x[max_dist[j]], y[max_dist[j]]);
				//= sqrt(x[ind18[i]] - x[max_dist[j]]* x[ind18[i]] - x[max_dist[j]]+ y[ind18[i]] - y[max_dist[j]]);
			}
		for (int i = 0; i < 2; i++)
			for (int j = 0; j < 2; j++)
			{
				dist45[i * 2 + j] = Dist(x[ind45[i]], y[ind45[i]], x[min_dist[j]], y[min_dist[j]]);
				//= sqrt(x[ind18[i]] - x[max_dist[j]]* x[ind18[i]] - x[max_dist[j]]+ y[ind18[i]] - y[max_dist[j]]);
			}
		//double deta_dist18;
		for (int k = 0; k < 4; k++)
			//for (int m = k + 1; m < 4;m++)
		{
			double d1 = 0;
			//double d2 = 0;
			if (d1 == 0 || d1 > dist18[k])
			{
				d1 = dist18[k];
			}
			bian2.bian[0] = d1;
			double d45 = 0;
			d45 += dist45[k];
			bian2.bian[1] = d45 / 4;
		}
	}




	return bian2;
}

void Cmp_bili(Bian &a, Bian &b)
{
	//double bias_00 = (abs((a.bian[0] * b.bian[0] - a.bian[1] * b.bian[1])));
	double bias_10 = abs((a.bian[0] * b.bian[1] - a.bian[1] * b.bian[0]) / sqrt((a.bian[0] * a.bian[0] + a.bian[1] * a.bian[1])*(b.bian[0] * b.bian[0] + b.bian[1] * b.bian[1])));
	//double bias_01 = (abs((a.bian[2] * b.bian[2] - a.bian[3] * b.bian[3])));
	double bias_11 = abs((a.bian[2] * b.bian[3] - a.bian[3] * b.bian[2]) / sqrt((a.bian[2] * a.bian[2] + a.bian[3] * a.bian[3])*(b.bian[2] * b.bian[2] + b.bian[3] * b.bian[3])));
	//double bias_0 = min(bias_00, bias_01);
	//double bias_1 = min(bias_10, bias_11);
	if (bias_10 < 1&&bias_11 < 1)
		printf("same,%f %f \n", bias_10,bias_11);
	else
		printf("diff,%f %f\n", bias_10, bias_11);
}

vector<Point2f> GetCorners(Mat srcImage)
{
	Mat copy = srcImage;
	vector<Point2f> corners;
	RNG  rng(12345);
	//auto it = corners.begin();
	goodFeaturesToTrack(srcImage, corners, 8, 0.01, 10);
	/*
	for (auto it = corners.begin(); it != corners.end(); it++)
	{
		circle(copy, *it, 1, Scalar(rng.uniform(0, 255), rng.uniform(0, 255), rng.uniform(0, 255)), 2, 8, 0);
	}
	*/
	//	imshow("1",copy);
	return corners;
}

